/*
* Nettle Source File.
* Copyright (C) 2013, Nettle (http://nttl.ru/).
*
* This file is part of NettleMath.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the GNU Lesser General Public License
* (LGPL) version 2.1 which accompanies this diLibution, and is available at
* http://www.gnu.org/licenses/lgpl-2.1.html
*
* This library is diLibuted in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* Description:
*
* History:
* Created by Sergey Serb, l-proger@yandex.ru
*/

#ifndef NMatrix4x4_h__
#define NMatrix4x4_h__

#include "NMatrixBase.h"
#include "NVector3.h"
#include "NVector4.h"
#include "NQuaternion.h"

namespace NettleMath{
    template<typename T>
    struct NMatrix4x4 : NMatrixBase<T, 4, 4>{
        union
        {
            T m16[16];
            T m44[4][4];
            /*struct //Anonymous structures supported byVC++, GCC but not standart feature
            {
            T _m00, _m01, _m02, _m03;
            T _m10, _m11, _m12, _m13;
            T _m20, _m21, _m22, _m23;
            T _m30, _m31, _m32, _m33;
            };
            struct
            {
            T _11, _12, _13, _14;
            T _21, _22, _23, _24;
            T _31, _32, _33, _34;
            T _41, _42, _43, _44;
            };*/
        };

        NMatrix4x4(){
        }

        NMatrix4x4(const NMatrix4x4<T>& u){
            for(int i=0; i<16; ++i){
                m16[i] = u.m16[i];
            }
        }
        NMatrix4x4(const NQuaternion<T>& q){
            *this = q;
        }
        NMatrix4x4 operator + (const NMatrix4x4<T>& u) const{
            NMatrix4x4<T> r;
            for ( int i=0; i<16; ++i ){
                r.m16[i] = m16[i] + u.m16[i];
            }
            return r;
        }

        NMatrix4x4 operator - (const NMatrix4x4& u) const{
            NMatrix4x4 r;
            for(int i=0; i<16; ++i){
                r.m16[i] = m16[i] - u.m16[i];
            }
            return r;
        }

        NMatrix4x4 operator * (const NMatrix4x4& u) const{
            NMatrix4x4 r;
            for(int i = 0; i < 4; ++i){
                const T* v = m16 + i * 4;
                r.m44[i][0] = v[0]*u.m44[0][0] + v[1]*u.m44[1][0] + v[2]*u.m44[2][0] + v[3]*u.m44[3][0];
                r.m44[i][1] = v[0]*u.m44[0][1] + v[1]*u.m44[1][1] + v[2]*u.m44[2][1] + v[3]*u.m44[3][1];
                r.m44[i][2] = v[0]*u.m44[0][2] + v[1]*u.m44[1][2] + v[2]*u.m44[2][2] + v[3]*u.m44[3][2];
                r.m44[i][3] = v[0]*u.m44[0][3] + v[1]*u.m44[1][3] + v[2]*u.m44[2][3] + v[3]*u.m44[3][3];
            }
            return r;
        }

        NMatrix4x4& operator += (const NMatrix4x4& u){
            for(int i = 0; i < 16; ++i){
                m16[i] += u.m16[i];
            }
            return *this;
        }

        NMatrix4x4& operator -= (const NMatrix4x4& u){
            for(int i = 0; i < 16; ++i){
                m16[i] -= u.m16[i];
            }
            return *this;
        }

        NMatrix4x4& operator *= (const NMatrix4x4& u){
            for(int i = 0; i < 4; ++i){
                T v[4];
                v[0] = m44[i][0];
                v[1] = m44[i][1];
                v[2] = m44[i][2];
                v[3] = m44[i][3];
                m44[i][0] = v[0]*u.m44[0][0] + v[1]*u.m44[1][0] + v[2]*u.m44[2][0] + v[3]*u.m44[3][0];
                m44[i][1] = v[0]*u.m44[0][1] + v[1]*u.m44[1][1] + v[2]*u.m44[2][1] + v[3]*u.m44[3][1];
                m44[i][2] = v[0]*u.m44[0][2] + v[1]*u.m44[1][2] + v[2]*u.m44[2][2] + v[3]*u.m44[3][2];
                m44[i][3] = v[0]*u.m44[0][3] + v[1]*u.m44[1][3] + v[2]*u.m44[2][3] + v[3]*u.m44[3][3];
            }
            return *this;
        }

        NMatrix4x4& operator *= (const T& s){
            for ( int i=0; i<16; ++i ){
                m16[i] *= s;
            }
            return *this;
        }

        void operator = (const NMatrix4x4& u){
            for ( int i=0; i< 16; ++i ){
                m16[i] = u.m16[i];
            }
        }

        bool operator == (const NMatrix4x4& u){
            for ( int i=0; i< 16; ++i ){
                if(m16[i] != u.m16[i])
                    return false;
            }
            return true;
        }

        bool operator != (const NMatrix4x4& u){
            for ( int i=0; i< 16; ++i ){
                if(m16[i] == u.m16[i])
                    return false;
            }
            return true;
        }

        void operator = (const NQuaternion<T>& q)
        {
            T sx = q.x * 2;
            T sy = q.y * 2;
            T sz = q.z * 2;
            T wx = q.w * sx;
            T wy = q.w * sy;
            T wz = q.w * sz;
            T xx = q.x * sx;
            T xy = q.x * sy;
            T xz = q.x * sz;
            T yy = q.y * sy;
            T yz = q.y * sz;
            T zz = q.z * sz;

            m44[0][0] = 1 - yy - zz;
            m44[0][1] = xy + wz;
            m44[0][2] = xz - wy;
            m44[0][3] = 0;
            m44[1][0] = xy - wz;
            m44[1][1] = 1 - xx - zz;
            m44[1][2] = yz + wx;
            m44[1][3] = 0;
            m44[2][0] = xz + wy;
            m44[2][1] = yz - wx;
            m44[2][2] = 1 - xx - yy;
            m44[2][3] =
            m44[3][0] =
            m44[3][1] =
            m44[3][2] = 0;
            m44[3][3] = 1;
        }

        NVector4<T>& operator [] (int index)
        {
            assert( index>=0 && index<=3 );
            return reinterpret_cast< NVector4<T>* >(this)[index];
        }

        const NVector4<T>& operator [] (int index) const
        {
            assert( index>=0 && index<=3 );
            return reinterpret_cast< const NVector4<T>* >(this)[index];
        }

        void Identity()
        {
            m44[0][0] = 1; m44[0][1] = 0; m44[0][2] = 0; m44[0][3] = 0;
            m44[1][0] = 0; m44[1][1] = 1; m44[1][2] = 0; m44[1][3] = 0;
            m44[2][0] = 0; m44[2][1] = 0; m44[2][2] = 1; m44[2][3] = 0;
            m44[3][0] = 0; m44[3][1] = 0; m44[3][2] = 0; m44[3][3] = 1;
        }

        void Scale(const T& x, const T& y, const T& z)
        {
            m44[0][0] = x; m44[0][1] = 0; m44[0][2] = 0; m44[0][3] = 0;
            m44[1][0] = 0; m44[1][1] = y; m44[1][2] = 0; m44[1][3] = 0;
            m44[2][0] = 0; m44[2][1] = 0; m44[2][2] = z; m44[2][3] = 0;
            m44[3][0] = 0; m44[3][1] = 0; m44[3][2] = 0; m44[3][3] = 1;
        }

        void Scale(const NVector3<T>& scale)
        {
            Scale(scale.x,scale.y,scale.z);
        }

        void Translate(const T& x, const T& y, const T& z)
        {
            m44[0][0] = 1; m44[0][1] = 0; m44[0][2] = 0; m44[0][3] = 0;
            m44[1][0] = 0; m44[1][1] = 1; m44[1][2] = 0; m44[1][3] = 0;
            m44[2][0] = 0; m44[2][1] = 0; m44[2][2] = 1; m44[2][3] = 0;
            m44[3][0] = x; m44[3][1] = y; m44[3][2] = z; m44[3][3] = 1;
        }

        void Translate(const NVector3<T>& translate)
        {
            Translate(translate.x,translate.y,translate.z);
        }

        void RotateX(const T& angle){
            T s = static_cast<T>(sin(angle));
            T c = static_cast<T>(cos(angle));

            m44[1][1] = c; m44[1][2] = s; 
            m44[2][1] =-s; m44[2][2] = c; 

            m44[0][0] =; 
            m44[3][3] = 1;

            m44[0][1] =
            m44[2][0] =
            m44[1][0] =
            m44[0][2] = 
            m44[0][3] =
            m44[3][0] = 
            m44[3][1] = 
            m44[3][2] =
            m44[1][3] =
            m44[2][3] = 0;
        }

        void RotateY(const T& angle)
        {
            T s = static_cast<T>(sin(angle));
            T c = static_cast<T>(cos(angle));

            m44[0][0] = c; m44[0][1] = 0; m44[0][2] =-s; m44[0][3] = 0;
            m44[1][0] = 0; m44[1][1] = 1; m44[1][2] = 0; m44[1][3] = 0;
            m44[2][0] = s; m44[2][1] = 0; m44[2][2] = c; m44[2][3] = 0;
            m44[3][0] = 0; m44[3][1] = 0; m44[3][2] = 0; m44[3][3] = 1;
        }

        void RotateZ(const T& angle)
        {
            T s = static_cast<T>(sin(angle));
            T c = static_cast<T>(cos(angle));

            m44[0][0] = c; m44[0][1] = s; m44[0][2] = 0; m44[0][3] = 0;
            m44[1][0] =-s; m44[1][1] = c; m44[1][2] = 0; m44[1][3] = 0;
            m44[2][0] = 0; m44[2][1] = 0; m44[2][2] = 1; m44[2][3] = 0;
            m44[3][0] = 0; m44[3][1] = 0; m44[3][2] = 0; m44[3][3] = 1;
        }

        void RotateXYZ(const T& pitch, const T& yaw, const T& roll)
        {
            T sp = static_cast<T>(sin(pitch));
            T cp = static_cast<T>(cos(pitch));
            T sy = static_cast<T>(sin(yaw));
            T cy = static_cast<T>(cos(yaw));
            T sr = static_cast<T>(sin(roll));
            T cr = static_cast<T>(cos(roll));
            T sysp = sy * sp;
            T sycp = sy * cp;

            m44[0][0] = cr * cy;
            m44[0][1] = sr * cy;
            m44[0][2] = -sy;
            m44[0][3] = 0;
            m44[1][0] = cr * sysp - sr * cp;
            m44[1][1] = sr * sysp + cr * cp;
            m44[1][2] = cy * sp;
            m44[1][3] = 0;
            m44[2][0] = cr * sycp + sr * sp;
            m44[2][1] = sr * sycp - cr * sp;
            m44[2][2] = cy * cp;
            m44[2][3] = 
            m44[3][0] = 
            m44[3][1] = 
            m44[3][2] = 0;
            m44[3][3] = 1;
        }

        void RotateAngleAxis(const T& angle, const NVector3<T>& axis)
        {
            T length2 = axis.LengthSquared();
            if ( length2 == 0 )
            {
                Identity();
                return;
            }

            NVector3<T> n = axis / static_cast<T>(sqrt(length2));
            T s = static_cast<T>(sin(angle));
            T c = static_cast<T>(cos(angle));
            T k = 1 - c;

            T xx = n.x * n.x * k + c;
            T yy = n.y * n.y * k + c;
            T zz = n.z * n.z * k + c;
            T xy = n.x * n.y * k;
            T yz = n.y * n.z * k;
            T zx = n.z * n.x * k;
            T xs = n.x * s;
            T ys = n.y * s;
            T zs = n.z * s;

            m44[0][0] = xx;
            m44[0][1] = xy + zs;
            m44[0][2] = zx - ys;
            m44[0][3] = 0;
            m44[1][0] = xy - zs;
            m44[1][1] = yy;
            m44[1][2] = yz + xs;
            m44[1][3] = 0;
            m44[2][0] = zx + ys;
            m44[2][1] = yz - xs;
            m44[2][2] = zz;
            m44[2][3] = 
            m44[3][0] = 
            m44[3][1] = 
            m44[3][2] = 0;
            m44[3][3] = 1;
        }

        void Transpose()
        {
            std::swap(m44[0][1],m44[1][0]);
            std::swap(m44[0][2],m44[2][0]);
            std::swap(m44[0][3],m44[3][0]);
            std::swap(m44[1][2],m44[2][1]);
            std::swap(m44[1][3],m44[3][1]);
            std::swap(m44[2][3],m44[3][2]);
        }

        void Transpose(const NMatrix4x4& u)
        {
            m44[0][0] = u.m44[0][0];
            m44[0][1] = u.m44[1][0];
            m44[0][2] = u.m44[2][0];
            m44[0][3] = u.m44[3][0];
            m44[1][0] = u.m44[0][1];
            m44[1][1] = u.m44[1][1];
            m44[1][2] = u.m44[2][1];
            m44[1][3] = u.m44[3][1];
            m44[2][0] = u.m44[0][2];
            m44[2][1] = u.m44[1][2];
            m44[2][2] = u.m44[2][2];
            m44[2][3] = u.m44[3][2];
            m44[3][0] = u.m44[0][3];
            m44[3][1] = u.m44[1][3];
            m44[3][2] = u.m44[2][3];
            m44[3][3] = u.m44[3][3];
        }

        void Adjoint()
        {
            T v00 = m44[1][1]*m44[2][2] - m44[1][2]*m44[2][1];
            T v01 = m44[2][1]*m44[0][2] - m44[2][2]*m44[0][1];
            T v02 = m44[0][1]*m44[1][2] - m44[0][2]*m44[1][1];
            T v10 = m44[1][2]*m44[2][0] - m44[1][0]*m44[2][2];
            T v11 = m44[2][2]*m44[0][0] - m44[2][0]*m44[0][2];
            T v12 = m44[0][2]*m44[1][0] - m44[0][0]*m44[1][2];
            T v20 = m44[1][0]*m44[2][1] - m44[1][1]*m44[2][0];
            T v21 = m44[2][0]*m44[0][1] - m44[2][1]*m44[0][0];
            T v22 = m44[0][0]*m44[1][1] - m44[0][1]*m44[1][0];
            T v30 = -(m44[0][0]*m44[3][0] + m44[1][0]*m44[3][1] + m44[2][0]*m44[3][2]);
            T v31 = -(m44[0][1]*m44[3][0] + m44[1][1]*m44[3][1] + m44[2][1]*m44[3][2]);
            T v32 = -(m44[0][2]*m44[3][0] + m44[1][2]*m44[3][1] + m44[2][2]*m44[3][2]);
            m44[0][0] = v00;
            m44[0][1] = v01;
            m44[0][2] = v02;
            m44[1][0] = v10;
            m44[1][1] = v11;
            m44[1][2] = v12;
            m44[2][0] = v20;
            m44[2][1] = v21;
            m44[2][2] = v22;
            m44[3][0] = v30;
            m44[3][1] = v31;
            m44[3][2] = v32;
        }

        void Adjoint(const NMatrix4x4& u)
        {
            m44[0][0] = u.m44[1][1]*u.m44[2][2] - u.m44[1][2]*u.m44[2][1];
            m44[0][1] = u.m44[2][1]*u.m44[0][2] - u.m44[2][2]*u.m44[0][1];
            m44[0][2] = u.m44[0][1]*u.m44[1][2] - u.m44[0][2]*u.m44[1][1];
            m44[0][3] = u.m44[0][3];
            m44[1][0] = u.m44[1][2]*u.m44[2][0] - u.m44[1][0]*u.m44[2][2];
            m44[1][1] = u.m44[2][2]*u.m44[0][0] - u.m44[2][0]*u.m44[0][2];
            m44[1][2] = u.m44[0][2]*u.m44[1][0] - u.m44[0][0]*u.m44[1][2];
            m44[1][3] = u.m44[1][3];
            m44[2][0] = u.m44[1][0]*u.m44[2][1] - u.m44[1][1]*u.m44[2][0];
            m44[2][1] = u.m44[2][0]*u.m44[0][1] - u.m44[2][1]*u.m44[0][0];
            m44[2][2] = u.m44[0][0]*u.m44[1][1] - u.m44[0][1]*u.m44[1][0];
            m44[2][3] = u.m44[2][3];
            m44[3][0] = -(u.m44[0][0]*u.m44[3][0] + u.m44[1][0]*u.m44[3][1] + u.m44[2][0]*u.m44[3][2]);
            m44[3][1] = -(u.m44[0][1]*u.m44[3][0] + u.m44[1][1]*u.m44[3][1] + u.m44[2][1]*u.m44[3][2]);
            m44[3][2] = -(u.m44[0][2]*u.m44[3][0] + u.m44[1][2]*u.m44[3][1] + u.m44[2][2]*u.m44[3][2]);
            m44[3][3] = u.m44[3][3];
        }

        void Inverse(bool affine = true)
        {
            if ( affine )
            {
                T s = 1 / GetDeterminant();
                T v00 = (m44[1][1]*m44[2][2] - m44[1][2]*m44[2][1]) * s;
                T v01 = (m44[2][1]*m44[0][2] - m44[2][2]*m44[0][1]) * s;
                T v02 = (m44[0][1]*m44[1][2] - m44[0][2]*m44[1][1]) * s;
                T v10 = (m44[1][2]*m44[2][0] - m44[1][0]*m44[2][2]) * s;
                T v11 = (m44[2][2]*m44[0][0] - m44[2][0]*m44[0][2]) * s;
                T v12 = (m44[0][2]*m44[1][0] - m44[0][0]*m44[1][2]) * s;
                T v20 = (m44[1][0]*m44[2][1] - m44[1][1]*m44[2][0]) * s;
                T v21 = (m44[2][0]*m44[0][1] - m44[2][1]*m44[0][0]) * s;
                T v22 = (m44[0][0]*m44[1][1] - m44[0][1]*m44[1][0]) * s;
                T v30 = -(v00*m44[3][0] + v10*m44[3][1] + v20*m44[3][2]);
                T v31 = -(v01*m44[3][0] + v11*m44[3][1] + v21*m44[3][2]);
                T v32 = -(v02*m44[3][0] + v12*m44[3][1] + v22*m44[3][2]);
                m44[0][0] = v00;
                m44[0][1] = v01;
                m44[0][2] = v02;
                m44[1][0] = v10;
                m44[1][1] = v11;
                m44[1][2] = v12;
                m44[2][0] = v20;
                m44[2][1] = v21;
                m44[2][2] = v22;
                m44[3][0] = v30;
                m44[3][1] = v31;
                m44[3][2] = v32;
            }
            else
            {
                // full 4x4 matrix inverse using Cramer's Rule
                // original code: Intel Corp., adapted to prmath

                // transpose matrix
                T src[16];
                for ( int i=0; i<4; ++i )
                {
                    src[i]      = m16[i*4];
                    src[i + 4]  = m16[i*4 + 1];
                    src[i + 8]  = m16[i*4 + 2];
                    src[i + 12] = m16[i*4 + 3];
                }

                // calculate pairs for first 8 elements (cofactors)
                T tmp[12]; // temp array for pairs
                tmp[0]  = src[10] * src[15];
                tmp[1]  = src[11] * src[14];
                tmp[2]  = src[9]  * src[15];
                tmp[3]  = src[11] * src[13];
                tmp[4]  = src[9]  * src[14];
                tmp[5]  = src[10] * src[13];
                tmp[6]  = src[8]  * src[15];
                tmp[7]  = src[11] * src[12];
                tmp[8]  = src[8]  * src[14];
                tmp[9]  = src[10] * src[12];
                tmp[10] = src[8]  * src[13];
                tmp[11] = src[9]  * src[12];

                // calculate first 8 elements (cofactors)
                m16[0] = (tmp[0] * src[5] + tmp[3] * src[6] + tmp[4]  * src[7]) - (tmp[1] * src[5] + tmp[2] * src[6] + tmp[5]  * src[7]);
                m16[1] = (tmp[1] * src[4] + tmp[6] * src[6] + tmp[9]  * src[7]) - (tmp[0] * src[4] + tmp[7] * src[6] + tmp[8]  * src[7]);
                m16[2] = (tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7]) - (tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7]);
                m16[3] = (tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6]) - (tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6]);
                m16[4] = (tmp[1] * src[1] + tmp[2] * src[2] + tmp[5]  * src[3]) - (tmp[0] * src[1] + tmp[3] * src[2] + tmp[4]  * src[3]);
                m16[5] = (tmp[0] * src[0] + tmp[7] * src[2] + tmp[8]  * src[3]) - (tmp[1] * src[0] + tmp[6] * src[2] + tmp[9]  * src[3]);
                m16[6] = (tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3]) - (tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3]);
                m16[7] = (tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2]) - (tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2]);

                // calculate pairs for second 8 elements (cofactors)
                tmp[0]  = src[2] * src[7];
                tmp[1]  = src[3] * src[6];
                tmp[2]  = src[1] * src[7];
                tmp[3]  = src[3] * src[5];
                tmp[4]  = src[1] * src[6];
                tmp[5]  = src[2] * src[5];
                tmp[6]  = src[0] * src[7];
                tmp[7]  = src[3] * src[4];
                tmp[8]  = src[0] * src[6];
                tmp[9]  = src[2] * src[4];
                tmp[10] = src[0] * src[5];
                tmp[11] = src[1] * src[4];

                // calculate second 8 elements (cofactors)
                m16[8]  = (tmp[0]  * src[13] + tmp[3]  * src[14] + tmp[4]  * src[15]) - (tmp[1]  * src[13] + tmp[2]  * src[14] + tmp[5]  * src[15]);
                m16[9]  = (tmp[1]  * src[12] + tmp[6]  * src[14] + tmp[9]  * src[15]) - (tmp[0]  * src[12] + tmp[7]  * src[14] + tmp[8]  * src[15]);
                m16[10] = (tmp[2]  * src[12] + tmp[7]  * src[13] + tmp[10] * src[15]) - (tmp[3]  * src[12] + tmp[6]  * src[13] + tmp[11] * src[15]);
                m16[11] = (tmp[5]  * src[12] + tmp[8]  * src[13] + tmp[11] * src[14]) - (tmp[4]  * src[12] + tmp[9]  * src[13] + tmp[10] * src[14]);
                m16[12] = (tmp[2]  * src[10] + tmp[5]  * src[11] + tmp[1]  * src[9]) - (tmp[4]  * src[11] + tmp[0]  * src[9]  + tmp[3]  * src[10]);
                m16[13] = (tmp[8]  * src[11] + tmp[0]  * src[8]  + tmp[7]  * src[10]) - (tmp[6]  * src[10] + tmp[9]  * src[11] + tmp[1]  * src[8]);
                m16[14] = (tmp[6]  * src[9]  + tmp[11] * src[11] + tmp[3]  * src[8]) - (tmp[10] * src[11] + tmp[2]  * src[8]  + tmp[7]  * src[9]);
                m16[15] = (tmp[10] * src[10] + tmp[4]  * src[8]  + tmp[9]  * src[9]) - (tmp[8]  * src[9]  + tmp[11] * src[10] + tmp[5]  * src[8]);

                // calculate determinant
                T det = src[0]*m16[0]+src[1]*m16[1]+src[2]*m16[2]+src[3]*m16[3];

                // calculate matrix inverse
                det = 1 / det;
                for ( int j=0; j<16; ++j )
                    m16[j] *= det;
            }
        }

        void Inverse(const NMatrix4x4& u, bool affine = true){
            if ( affine ){
                T s = static_cast<T>(1) / u.GetDeterminant();
                m44[0][0] = (u.m44[1][1]*u.m44[2][2] - u.m44[1][2]*u.m44[2][1]) * s;
                m44[0][1] = (u.m44[2][1]*u.m44[0][2] - u.m44[2][2]*u.m44[0][1]) * s;
                m44[0][2] = (u.m44[0][1]*u.m44[1][2] - u.m44[0][2]*u.m44[1][1]) * s;
                m44[0][3] =  u.m44[0][3];
                m44[1][0] = (u.m44[1][2]*u.m44[2][0] - u.m44[1][0]*u.m44[2][2]) * s;
                m44[1][1] = (u.m44[2][2]*u.m44[0][0] - u.m44[2][0]*u.m44[0][2]) * s;
                m44[1][2] = (u.m44[0][2]*u.m44[1][0] - u.m44[0][0]*u.m44[1][2]) * s;
                m44[1][3] =  u.m44[1][3];
                m44[2][0] = (u.m44[1][0]*u.m44[2][1] - u.m44[1][1]*u.m44[2][0]) * s;
                m44[2][1] = (u.m44[2][0]*u.m44[0][1] - u.m44[2][1]*u.m44[0][0]) * s;
                m44[2][2] = (u.m44[0][0]*u.m44[1][1] - u.m44[0][1]*u.m44[1][0]) * s;
                m44[2][3] =  u.m44[2][3];
                m44[3][0] =  -(m44[0][0]*u.m44[3][0] + m44[1][0]*u.m44[3][1] + m44[2][0]*u.m44[3][2]);
                m44[3][1] =  -(m44[0][1]*u.m44[3][0] + m44[1][1]*u.m44[3][1] + m44[2][1]*u.m44[3][2]);
                m44[3][2] =  -(m44[0][2]*u.m44[3][0] + m44[1][2]*u.m44[3][1] + m44[2][2]*u.m44[3][2]);
                m44[3][3] =  u.m44[3][3];
            }
            else
            {
                // full 4x4 matrix inverse using Cramer's Rule
                // original code: Intel Corp., adapted to prmath

                // transpose matrix
                T src[16];
                for ( int i=0; i<4; ++i )
                {
                    src[i]      = u.m16[i*4];
                    src[i + 4]  = u.m16[i*4 + 1];
                    src[i + 8]  = u.m16[i*4 + 2];
                    src[i + 12] = u.m16[i*4 + 3];
                }

                // calculate pairs for first 8 elements (cofactors)
                T tmp[12]; // temp array for pairs
                tmp[0]  = src[10] * src[15];
                tmp[1]  = src[11] * src[14];
                tmp[2]  = src[9]  * src[15];
                tmp[3]  = src[11] * src[13];
                tmp[4]  = src[9]  * src[14];
                tmp[5]  = src[10] * src[13];
                tmp[6]  = src[8]  * src[15];
                tmp[7]  = src[11] * src[12];
                tmp[8]  = src[8]  * src[14];
                tmp[9]  = src[10] * src[12];
                tmp[10] = src[8]  * src[13];
                tmp[11] = src[9]  * src[12];

                // calculate first 8 elements (cofactors)
                m16[0] = (tmp[0] * src[5] + tmp[3] * src[6] + tmp[4]  * src[7]) - (tmp[1] * src[5] + tmp[2] * src[6] + tmp[5]  * src[7]);
                m16[1] = (tmp[1] * src[4] + tmp[6] * src[6] + tmp[9]  * src[7]) - (tmp[0] * src[4] + tmp[7] * src[6] + tmp[8]  * src[7]);
                m16[2] = (tmp[2] * src[4] + tmp[7] * src[5] + tmp[10] * src[7]) - (tmp[3] * src[4] + tmp[6] * src[5] + tmp[11] * src[7]);
                m16[3] = (tmp[5] * src[4] + tmp[8] * src[5] + tmp[11] * src[6]) - (tmp[4] * src[4] + tmp[9] * src[5] + tmp[10] * src[6]);
                m16[4] = (tmp[1] * src[1] + tmp[2] * src[2] + tmp[5]  * src[3]) - (tmp[0] * src[1] + tmp[3] * src[2] + tmp[4]  * src[3]);
                m16[5] = (tmp[0] * src[0] + tmp[7] * src[2] + tmp[8]  * src[3]) - (tmp[1] * src[0] + tmp[6] * src[2] + tmp[9]  * src[3]);
                m16[6] = (tmp[3] * src[0] + tmp[6] * src[1] + tmp[11] * src[3]) - (tmp[2] * src[0] + tmp[7] * src[1] + tmp[10] * src[3]);
                m16[7] = (tmp[4] * src[0] + tmp[9] * src[1] + tmp[10] * src[2]) - (tmp[5] * src[0] + tmp[8] * src[1] + tmp[11] * src[2]);

                // calculate pairs for second 8 elements (cofactors)
                tmp[0]  = src[2] * src[7];
                tmp[1]  = src[3] * src[6];
                tmp[2]  = src[1] * src[7];
                tmp[3]  = src[3] * src[5];
                tmp[4]  = src[1] * src[6];
                tmp[5]  = src[2] * src[5];
                tmp[6]  = src[0] * src[7];
                tmp[7]  = src[3] * src[4];
                tmp[8]  = src[0] * src[6];
                tmp[9]  = src[2] * src[4];
                tmp[10] = src[0] * src[5];
                tmp[11] = src[1] * src[4];

                // calculate second 8 elements (cofactors)
                m16[8]  = (tmp[0]  * src[13] + tmp[3]  * src[14] + tmp[4]  * src[15]) - (tmp[1]  * src[13] + tmp[2]  * src[14] + tmp[5]  * src[15]);
                m16[9]  = (tmp[1]  * src[12] + tmp[6]  * src[14] + tmp[9]  * src[15]) - (tmp[0]  * src[12] + tmp[7]  * src[14] + tmp[8]  * src[15]);
                m16[10] = (tmp[2]  * src[12] + tmp[7]  * src[13] + tmp[10] * src[15]) - (tmp[3]  * src[12] + tmp[6]  * src[13] + tmp[11] * src[15]);
                m16[11] = (tmp[5]  * src[12] + tmp[8]  * src[13] + tmp[11] * src[14]) - (tmp[4]  * src[12] + tmp[9]  * src[13] + tmp[10] * src[14]);
                m16[12] = (tmp[2]  * src[10] + tmp[5]  * src[11] + tmp[1]  * src[9]) - (tmp[4]  * src[11] + tmp[0]  * src[9]  + tmp[3]  * src[10]);
                m16[13] = (tmp[8]  * src[11] + tmp[0]  * src[8]  + tmp[7]  * src[10]) - (tmp[6]  * src[10] + tmp[9]  * src[11] + tmp[1]  * src[8]);
                m16[14] = (tmp[6]  * src[9]  + tmp[11] * src[11] + tmp[3]  * src[8]) - (tmp[10] * src[11] + tmp[2]  * src[8]  + tmp[7]  * src[9]);
                m16[15] = (tmp[10] * src[10] + tmp[4]  * src[8]  + tmp[9]  * src[9]) - (tmp[8]  * src[9]  + tmp[11] * src[10] + tmp[5]  * src[8]);

                // calculate determinant
                T det = src[0]*m16[0]+src[1]*m16[1]+src[2]*m16[2]+src[3]*m16[3];

                // calculate matrix inverse
                det = 1 / det;
                for ( int j=0; j<16; ++j )
                    m16[j] *= det;
            }
        }

        void Inverse(const NQuaternion<T>& q)
        {
            // quaternion inverse
            T n = -1 / q.Norm();
            T qx = q.x * n;
            T qy = q.y * n;
            T qz = q.z * n;
            T qw = q.w * -n;

            // quaternion to matrix
            T sx = qx * 2;
            T sy = qy * 2;
            T sz = qz * 2;
            T wx = qw * sx;
            T wy = qw * sy;
            T wz = qw * sz;
            T xx = qx * sx;
            T xy = qx * sy;
            T xz = qx * sz;
            T yy = qy * sy;
            T yz = qy * sz;
            T zz = qz * sz;

            m44[0][0] = 1 - yy - zz;
            m44[0][1] = xy + wz;
            m44[0][2] = xz - wy;
            m44[0][3] = 0;
            m44[1][0] = xy - wz;
            m44[1][1] = 1 - xx - zz;
            m44[1][2] = yz + wx;
            m44[1][3] = 0;
            m44[2][0] = xz + wy;
            m44[2][1] = yz - wx;
            m44[2][2] = 1 - xx - yy;
            m44[2][3] =
            m44[3][0] =
            m44[3][1] =
            m44[3][2] = 0;
            m44[3][3] = 1;
        }

        void OrthoNormalize()
        {
            NVector3<T> x = GetX();
            NVector3<T> y = GetY();

            x = normalize(x);
            y -= x * dot(x,y);
            y = normalize(y);
            z = cross(x,y);

            SetX(x);
            SetY(y);
            SetZ(z);
        }

        T GetDeterminant() const{
            return
                m44[0][0] * m44[1][1] * m44[2][2] +
                m44[0][1] * m44[1][2] * m44[2][0] +
                m44[0][2] * m44[1][0] * m44[2][1] -
                m44[0][2] * m44[1][1] * m44[2][0] -
                m44[0][1] * m44[1][0] * m44[2][2] -
                m44[0][0] * m44[1][2] * m44[2][1];
        }

        void SetX(const T& xx, const T& xy, const T& xz)
        {
            m44[0][0] = xx;
            m44[0][1] = xy;
            m44[0][2] = xz;
        }

        void SetY(const T& yx, const T& yy, const T& yz)
        {
            m44[1][0] = yx;
            m44[1][1] = yy;
            m44[1][2] = yz;
        }

        void SetZ(const T& zx, const T& zy, const T& zz)
        {
            m44[2][0] = zx;
            m44[2][1] = zy;
            m44[2][2] = zz;
        }

        void SetT(const T& tx, const T& ty, const T& tz)
        {
            m44[3][0] = tx;
            m44[3][1] = ty;
            m44[3][2] = tz;
        }

        void SetX(const NVector3<T>& x)
        {
            m44[0][0] = x.x;
            m44[0][1] = x.y;
            m44[0][2] = x.z;
        }

        void SetY(const NVector3<T>& y)
        {
            m44[1][0] = y.x;
            m44[1][1] = y.y;
            m44[1][2] = y.z;
        }

        void SetZ(const NVector3<T>& z)
        {
            m44[2][0] = z.x;
            m44[2][1] = z.y;
            m44[2][2] = z.z;
        }

        void SetT(const NVector3<T>& t)
        {
            m44[3][0] = t.x;
            m44[3][1] = t.y;
            m44[3][2] = t.z;
        }

        NVector3<T>& GetX()
        {
            return *reinterpret_cast<NVector3<T>*>(m16+0);
        }

        NVector3<T>& GetY()
        {
            return *reinterpret_cast<NVector3<T>*>(m16+4);
        }

        NVector3<T>& GetZ()
        {
            return *reinterpret_cast<NVector3<T>*>(m16+8);
        }

        NVector3<T>& GetT()
        {
            return *reinterpret_cast<NVector3<T>*>(m16+12);
        }

        const NVector3<T>& GetX() const
        {
            return *reinterpret_cast<const NVector3<T>*>(m16+0);
        }

        const NVector3<T>& GetY() const
        {
            return *reinterpret_cast<const NVector3<T>*>(m16+4);
        }

        const NVector3<T>& GetZ() const
        {
            return *reinterpret_cast<const NVector3<T>*>(m16+8);
        }

        const NVector3<T>& GetT() const
        {
            return *reinterpret_cast<const NVector3<T>*>(m16+12);
        }
    };

    //inline matrix operators
    //left - row vector, right - column vector

    template <typename T>
    inline void operator *= (NVector3<T>& v, const NMatrix4x4<T>& m)
    {
        v =	NVector3<T>(
            v.x*m.m44[0][0] + v.y*m.m44[1][0] + v.z*m.m44[2][0] + m.m44[3][0],
            v.x*m.m44[0][1] + v.y*m.m44[1][1] + v.z*m.m44[2][1] + m.m44[3][1],
            v.x*m.m44[0][2] + v.y*m.m44[1][2] + v.z*m.m44[2][2] + m.m44[3][2]);
    }

    template <typename T>
    inline void operator *= (NVector4<T>& v, const NMatrix4x4<T>& m)
    {
        v = NVector4<T>(
            v.x*m.m44[0][0] + v.y*m.m44[1][0] + v.z*m.m44[2][0] + v.w*m.m44[3][0],
            v.x*m.m44[0][1] + v.y*m.m44[1][1] + v.z*m.m44[2][1] + v.w*m.m44[3][1],
            v.x*m.m44[0][2] + v.y*m.m44[1][2] + v.z*m.m44[2][2] + v.w*m.m44[3][2],
            v.x*m.m44[0][3] + v.y*m.m44[1][3] + v.z*m.m44[2][3] + v.w*m.m44[3][3]);
    }

    template <typename T>
    inline NVector3<T> operator * (const NVector3<T>& v, const NMatrix4x4<T>& m)
    {
        return NVector3<T>(
            v.x*m.m44[0][0] + v.y*m.m44[1][0] + v.z*m.m44[2][0] + m.m44[3][0],
            v.x*m.m44[0][1] + v.y*m.m44[1][1] + v.z*m.m44[2][1] + m.m44[3][1],
            v.x*m.m44[0][2] + v.y*m.m44[1][2] + v.z*m.m44[2][2] + m.m44[3][2]);
    }

    //TODO: test me
    template <typename T>
    inline NVector3<T> operator * (const NMatrix4x4<T>& m, const NVector3<T>& v)
    {
        return NVector3<T>(
            v.x*m.m44[0][0] + v.y*m.m44[0][1] + v.z*m.m44[0][2] + m.m44[0][3],
            v.x*m.m44[1][0] + v.y*m.m44[1][1] + v.z*m.m44[1][2] + m.m44[1][3],
            v.x*m.m44[2][0] + v.y*m.m44[2][1] + v.z*m.m44[2][2] + m.m44[2][3]);
    }

    template <typename T>
    inline NVector4<T> operator * (const NVector4<T>& v, const NMatrix4x4<T>& m)
    {
        return NVector4<T>(
            v.x*m.m44[0][0] + v.y*m.m44[1][0] + v.z*m.m44[2][0] + v.w*m.m44[3][0],
            v.x*m.m44[0][1] + v.y*m.m44[1][1] + v.z*m.m44[2][1] + v.w*m.m44[3][1],
            v.x*m.m44[0][2] + v.y*m.m44[1][2] + v.z*m.m44[2][2] + v.w*m.m44[3][2],
            v.x*m.m44[0][3] + v.y*m.m44[1][3] + v.z*m.m44[2][3] + v.w*m.m44[3][3]);
    }

    //TODO: test me
    template <typename T>
    inline NVector4<T> operator * (const NMatrix4x4<T>& m, const NVector4<T>& v)
    {
        return NVector4<T>(
            v.x*m.m44[0][0] + v.y*m.m44[0][1] + v.z*m.m44[0][2] + v.w*m.m44[0][3],
            v.x*m.m44[1][0] + v.y*m.m44[1][1] + v.z*m.m44[1][2] + v.w*m.m44[1][3],
            v.x*m.m44[2][0] + v.y*m.m44[2][1] + v.z*m.m44[2][2] + v.w*m.m44[2][3],
            v.x*m.m44[3][0] + v.y*m.m44[3][1] + v.z*m.m44[3][2] + v.w*m.m44[3][3]);
    }

    typedef NMatrix4x4<float> float4x4;
    typedef NMatrix4x4<double> double4x4;
}
#endif // NMatrix4x4_h__